#include <linux/types.h>
#include <linux/stddef.h>
#include <linux/unistd.h>
#include <linux/module.h>
#include <linux/crypto.h>
#include <linux/version.h>
#include <linux/kernel.h>
#include <linux/string.h>
#include <linux/mm.h>
#include <linux/slab.h>
#include <linux/sched.h>
#include <linux/in.h>
#include <linux/netdevice.h>
#include <linux/file.h>
#include <linux/fs.h>
#include <linux/proc_fs.h>
#include <linux/namei.h>
#include <linux/dirent.h>
#include <linux/kobject.h>
#include <linux/ip.h>
#include <linux/netfilter.h>
#include <linux/netfilter_ipv4.h>
#include <linux/skbuff.h>
#include <linux/if_pppox.h>
#include <linux/list.h>
#include <linux/ptrace.h>
#include <linux/spinlock.h>
#include <linux/tty.h>
#include <linux/tty_driver.h>
#include <net/tcp.h>
#include <asm/processor.h>
#include <asm/uaccess.h>
#include <asm/unistd.h>


static int dump_buf(unsigned char *buf, int len)
{
    unsigned char *p;
    int i;
    for (p = buf, i = 1; i <= len; ++i, ++p) {
        if (i % 0x10 == 0 || i == len)
            printk(KERN_DEBUG "%02x\n", *p);            
        else
            printk(KERN_DEBUG "%02x ", *p);
    }
    return (0);
}

static int dump_mypppoe_skb(struct sk_buff *skb)
{
    unsigned char *p;
    int len;
    p = skb->head;
    len = skb->end - skb->head;
    return dump_buf(p, len);
}


typedef int (*parse_func)(struct sk_buff *skb, struct net_device *dev, struct sk_buff *new_buf);

static int parse_padi(struct sk_buff *skb, struct net_device *dev, struct sk_buff *new_skb)
{
    struct pppoe_tag *new_tag;
    struct pppoe_tag *tag = (struct pppoe_tag *)skb->data;
    
    if (skb->len < sizeof(struct pppoe_tag))
        return (-2);

    if (skb_pull(skb, htons(tag->tag_len) + sizeof(struct pppoe_tag)) == NULL)
        return (-2);
    
    new_tag = (struct pppoe_tag *)skb_put(new_skb, htons(tag->tag_len) + sizeof(struct pppoe_tag));

    printk(KERN_DEBUG "tag_type = %04x, len = %d\n", htons(tag->tag_type), htons(tag->tag_len));

    switch(tag->tag_type)
    {
        case PTT_SRV_NAME:
        case PTT_RELAY_SID:
        case PTT_HOST_UNIQ:
            memcpy(new_tag, tag, htons(tag->tag_len) + sizeof(struct pppoe_tag));
            return (sizeof(struct pppoe_tag) + htons(tag->tag_len));
    }
    return (-1);
}

static int parse_padr(struct sk_buff *skb, struct net_device *dev, struct sk_buff *new_skb)
{
    struct pppoe_tag *new_tag = NULL;
    struct pppoe_tag *tag = (struct pppoe_tag *)skb->data;
    if (skb->len < sizeof(struct pppoe_tag))
        return (-2);

    if (skb_pull(skb, htons(tag->tag_len) + sizeof(struct pppoe_tag)) == NULL)
        return (-2);

    new_tag = (struct pppoe_tag *)skb_put(new_skb, htons(tag->tag_len) + sizeof(struct pppoe_tag));
    
    switch(tag->tag_type)
    {
        case PTT_SRV_NAME:
        case PTT_RELAY_SID:
        case PTT_HOST_UNIQ:
            memcpy(new_tag, tag, htons(tag->tag_len) + sizeof(struct pppoe_tag));
            return (sizeof(struct pppoe_tag) + htons(tag->tag_len));
    }
    return (-1);
}

static int parse_pppoe_tags(struct sk_buff *skb, struct net_device *dev, struct sk_buff *new_skb, parse_func func)
{
    int len = 0;
    int ret;
    for (;;) {
        if ((ret = func(skb, dev, new_skb)) < 0)
            break;
        len += ret;
    }
    return (len);
}

/*
 * return 0 means OK
 * others means FAIL
 */ 
static int check_tag(void *buf, short buf_len, short tag_type,const char *tag_data,const short tag_len)
{
    struct pppoe_tag *tag = (struct pppoe_tag *)buf;
    buf_len -= (htons(tag->tag_len) + sizeof(struct pppoe_tag));    
    while (buf_len >= 0 && htons(tag->tag_type) != 0) {
        if (tag_type == tag->tag_type) {
            if (!tag_data)
                return (0);
            
            if (htons(tag->tag_len) == tag_len &&
                memcmp(tag->tag_data, tag_data, tag_len) == 0) {
                return (0);
            }
        }
        tag = (struct pppoe_tag *)((char *)tag + htons(tag->tag_len) + sizeof(struct pppoe_tag));
        buf_len -= (htons(tag->tag_len) + sizeof(struct pppoe_tag));
    }
    return (-1);
}

const char *PPPOE_ACNAME = "pppoe-l2tp";
const int PPPOE_ACNAME_LEN = 11;

#ifdef PPPOE_MD5_COOKIE
static int pppoe_cookie_len;
static struct digest_alg *digest;

#define MD5_BLOCK_WORDS		16
#define MD5_HASH_WORDS		4
struct md5_ctx {
	u32 hash[MD5_HASH_WORDS];
	u32 block[MD5_BLOCK_WORDS];
	u64 byte_count;
};
extern struct crypto_alg *crypto_alg_mod_lookup(const char *name);

static unsigned char testbuf[] = {
    0x19, 0xc9, 0xd3, 0x85, 0x12, 0x34, 0x91, 0xbd,
    0x3b, 0x2a, 0xb4, 0x09, 0xa9, 0x5f, 0x8d, 0xe2
};

static int gen_pppoe_cookie(void *src_addr, void *dev_addr, void *out)
{
    struct md5_ctx ctx;
    memset(&ctx, 0, sizeof(ctx));
    digest->dia_init(&ctx);
    digest->dia_update(&ctx, src_addr, ETH_ALEN);
    digest->dia_update(&ctx, dev_addr, MAX_ADDR_LEN);
    digest->dia_final(&ctx, out);

/*    memcpy(out, testbuf, sizeof(testbuf)); */

    return (0);
}
#endif


static int handle_padi(struct sk_buff *skb, struct net_device *dev)
{
    struct pppoe_tag *acname;
#ifdef PPPOE_MD5_COOKIE
    struct pppoe_tag *cookie;
#endif
    struct pppoe_hdr *ph;
    struct sk_buff *new_skb = NULL;
    struct ethhdr *eth = (struct ethhdr *)skb->mac.raw;
    int tag_len = 0;

    printk(KERN_DEBUG "enter %s\n", __FUNCTION__);    

    ph = (struct pppoe_hdr *)skb->data;
    if (check_tag(ph->tag, htons(ph->length), PTT_SRV_NAME, NULL, 0) != 0) {
		goto out;
    }

    skb_pull(skb, sizeof(struct pppoe_hdr));

#ifdef PPPOE_MD5_COOKIE    
    new_skb = alloc_skb(skb->truesize + PPPOE_ACNAME_LEN + sizeof(struct pppoe_tag) +
        pppoe_cookie_len + sizeof(struct pppoe_tag), GFP_ATOMIC);
#else
    new_skb = alloc_skb(skb->truesize + PPPOE_ACNAME_LEN + sizeof(struct pppoe_tag), GFP_ATOMIC);    
#endif    

    if (!new_skb)
        goto out;

	skb_reserve(new_skb, LL_RESERVED_SPACE(dev));

	if (dev->hard_header &&
	    dev->hard_header(new_skb, dev, ETH_P_PPP_DISC,
            eth->h_source, dev->dev_addr, new_skb->len) < 0)
		goto out;

    ph = (struct pppoe_hdr *)skb_put(new_skb, sizeof(struct pppoe_hdr));

    new_skb->nh.raw = new_skb->data;
	new_skb->dev = dev;
	new_skb->protocol = htons(ETH_P_PPP_DISC);

    
    ph->ver = ph->type = 0x01;
    ph->code = PADO_CODE;
    ph->sid = 0;
/*    ph->length = 4; */
/*    dump_buf(ph, sizeof(struct pppoe_hdr)); */
/*    printk(KERN_DEBUG "ph = %p, new_skb->data = %p\n", ph, new_skb->data);  */
    
    acname = (struct pppoe_tag *)skb_put(new_skb, sizeof(struct pppoe_tag) + PPPOE_ACNAME_LEN);
    tag_len += sizeof(struct pppoe_tag) + PPPOE_ACNAME_LEN;
    acname->tag_type = PTT_AC_NAME;
    acname->tag_len = htons(PPPOE_ACNAME_LEN);
    memcpy(acname->tag_data, PPPOE_ACNAME, PPPOE_ACNAME_LEN);
#ifdef PPPOE_MD5_COOKIE        
    cookie = (struct pppoe_tag *)skb_put(new_skb, sizeof(struct pppoe_tag) + pppoe_cookie_len);
    tag_len += sizeof(struct pppoe_tag) + pppoe_cookie_len;
    cookie->tag_type = PTT_AC_COOKIE;
    cookie->tag_len = htons(digest->dia_digestsize);
    gen_pppoe_cookie(eth->h_source, dev->dev_addr, cookie->tag_data);
#endif
    printk(KERN_DEBUG "tag_len = %d\n", tag_len);
    
    tag_len += parse_pppoe_tags(skb, dev, new_skb, parse_padi);
    
/*    dump_buf(ph, sizeof(struct pppoe_hdr)); */

    printk(KERN_DEBUG "tag_len = %d\n", tag_len);    
    ph->length = htons(tag_len);
    
    dev_queue_xmit(new_skb);
    return (0);
out:
    if (new_skb)
        kfree_skb(new_skb);
    return (-1);
}

static int handle_padr(struct sk_buff *skb, struct net_device *dev)
{
    struct sk_buff *new_skb = NULL;
	struct pppoe_hdr *ph;    
    struct ethhdr *eth = (struct ethhdr *)skb->mac.raw;
    int tag_len = 0;
#ifdef PPPOE_MD5_COOKIE
    void *cookie_buf = kmalloc(pppoe_cookie_len, GFP_ATOMIC);
    if (!cookie_buf)
        goto out;
    if (gen_pppoe_cookie(eth->h_source, dev->dev_addr, cookie_buf) != 0)
        goto out;

    dump_buf(cookie_buf, pppoe_cookie_len);
    
#endif
    printk(KERN_DEBUG "enter %s\n", __FUNCTION__);    

    ph = (struct pppoe_hdr *)skb->data;
    if (check_tag(ph->tag, htons(ph->length), PTT_SRV_NAME, NULL, 0) != 0) {
        printk(KERN_DEBUG "check srv name fail %s\n", __FUNCTION__);                    
        goto out;
    }

#ifdef PPPOE_MD5_COOKIE
    if (check_tag(ph->tag, htons(ph->length), PTT_AC_COOKIE, cookie_buf, pppoe_cookie_len) != 0) { 
        printk(KERN_DEBUG "check cookie fail %s\n", __FUNCTION__);
        goto out;
    }
    if (cookie_buf) {
        kfree(cookie_buf);
        cookie_buf = NULL;
    }
#else    
    if (check_tag(ph->tag, htons(ph->length), PTT_AC_NAME, PPPOE_ACNAME, PPPOE_ACNAME_LEN) != 0) {
        goto out;
    }
#endif    
    
    skb_pull(skb, sizeof(struct pppoe_hdr));
    new_skb = alloc_skb(skb->truesize, GFP_ATOMIC);
    if (!new_skb)
        goto out;
	skb_reserve(new_skb, LL_RESERVED_SPACE(dev));

    if (dev->hard_header &&
	    dev->hard_header(new_skb, dev, ETH_P_PPP_DISC,
            eth->h_source, dev->dev_addr, new_skb->len) < 0)
		goto out;

    ph = (struct pppoe_hdr *)skb_put(new_skb, sizeof(struct pppoe_hdr));    
    new_skb->nh.raw = new_skb->data;
	new_skb->dev = dev;
	new_skb->protocol = htons(ETH_P_PPP_DISC);
    ph->ver = ph->type = 0x01;
    ph->code = PADS_CODE;
    ph->sid = 0;

    tag_len += parse_pppoe_tags(skb, dev, new_skb, parse_padr);

    ph->length = htons(tag_len);
    dev_queue_xmit(new_skb);
    printk(KERN_DEBUG "leave %s\n", __FUNCTION__);            
    return (0);
out:
    printk(KERN_DEBUG "out %s\n", __FUNCTION__);        
    if (new_skb)
        kfree_skb(new_skb);
#ifdef PPPOE_MD5_COOKIE
    if (cookie_buf)
        kfree(cookie_buf);
#endif
    return (-1);    
}

int my_pppoe(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt)
{
	struct pppoe_hdr *ph;
	ph = (struct pppoe_hdr *) skb->nh.raw;

    printk(KERN_DEBUG "enter %s\n", __FUNCTION__);    
    
    switch (ph->code)
    {
        case PADI_CODE:
            handle_padi(skb, dev);
            break;
        case PADR_CODE:
            handle_padr(skb, dev);            
            break;
        case PADT_CODE:
            break;
        default:
            printk(KERN_DEBUG "code[%d] err\n", ph->code);
    }

    return (0);
}


typedef int (*pppoe_hook)(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt);
extern pppoe_hook test_pppoe_hook;

#ifdef PPPOE_MD5_COOKIE

/*
void test_cookie(void)
{
    char buf[1024];
    gen_pppoe_cookie("123456", "123.123.123.123", buf);
    dump_buf(buf, pppoe_cookie_len);
}
*/

static int init_pppoe_cookie(void)
{
    struct crypto_alg *alg;
    alg = crypto_alg_mod_lookup("md5");
	if (alg == NULL) {
        pppoe_cookie_len = 0;
        digest = NULL;
        return (-1);
    }
    digest = &alg->cra_digest;
    pppoe_cookie_len = digest->dia_digestsize;
    return (0);
}
#endif

static int test_send_udp_buf(void)
{
/* static int udp_push_pending_frames(struct sock *sk, struct udp_sock *up) */    
/*
 * 	struct sk_buff *skb;
 * 	struct udphdr *uh;
 * 	    skb = alloc_skb(100, GFP_ATOMIC);
 * 		uh = skb->h.uh;
 * err = ip_push_pending_frames(sk); */
    return (0);
}

int mypppoe_init(void)
{
#ifdef PPPOE_MD5_COOKIE
    init_pppoe_cookie();    
/*
    test_cookie();
    return (0);
*/
#endif

    printk(KERN_DEBUG "%s init\n", __FUNCTION__);
    printk("test_pppoe_hook = %p\n", test_pppoe_hook);


    test_pppoe_hook = my_pppoe;
    return 0;
}

void mypppoe_exit(void)
{
	printk(KERN_DEBUG "%s exit\n", __FUNCTION__);
    test_pppoe_hook = NULL;
}

module_init(mypppoe_init);
module_exit(mypppoe_exit);


MODULE_LICENSE("GPL");
MODULE_AUTHOR("tpl.a1406@gmail.com");


